#include <avr/pgmspace.h>
//#include <Wire.h>

#include "BMA222E.h"

BMA222E::BMA222E(void)
{

}

void BMA222E::begin(void)
{
	Wire.begin();
	dev_addr = BMA222E_ADDR_W>>1;	
	setPowerMode(PM_NORMAL);
}

void BMA222E::setPowerMode(uint8_t mode)
{
	uint8_t low_power = 0x00;
	uint8_t lpw        = 0x00;

	switch(mode) {
	case PM_NORMAL:
		//default value
		break;
	case PM_DEEP_SUSPEND:
		lpw = 1 << 5;
		break;
	case PM_LOW_POWER1:
		lpw = 1 << 6;
		break;
	case PM_LOW_POWER2:
		lpw = 1 << 6;
		low_power = 1 << 6;
		break;
	case PM_SUSPEND:
		lpw = 1 << 7;
		break;
	default:
		return;
	}

	write_data(0x11, lpw);
	write_data(0x12, low_power);
}

void BMA222E::getRawXYZ(int8_t *x_raw, int8_t *y_raw, int8_t *z_raw)
{
	uint8_t xyz[6];

	read_data(0x02, xyz, 6);
	*x_raw = xyz[1];
	*y_raw = xyz[3];
	*z_raw = xyz[5];
}

void BMA222E::getXYZ(int16_t *x, int16_t *y, int16_t *z)
{
	int8_t x_raw,y_raw,z_raw;
	getRawXYZ(&x_raw, &y_raw, &z_raw);
	*x = ((int8_t)x_raw) * 156 / 10;
	*y = ((int8_t)y_raw) * 156 / 10;
	*z = ((int8_t)z_raw) * 156 / 10;
}

int16_t BMA222E::getX(void)
{
	uint8_t raw[2];

	read_data(0x02, raw, 2);
	return ((int8_t)raw[1]) * 156 / 10;
}

int16_t BMA222E::getY(void)
{
	uint8_t raw[2];

	read_data(0x04, raw, 2);
	return ((int8_t)raw[1]) * 156 / 10;

}

int16_t BMA222E::getZ(void)
{
	uint8_t raw[2];

	read_data(0x06, raw, 2);
	return ((int8_t)raw[1]) * 156 / 10;
}

void BMA222E::write_data(uint8_t reg, uint8_t data)
{
	Wire.beginTransmission(dev_addr);
	Wire.write(reg);
	Wire.write(data);
	Wire.endTransmission();
}

void BMA222E::read_data(uint8_t start_reg, uint8_t *buffer, uint8_t len)
{
	uint8_t index=0;

	Wire.beginTransmission(dev_addr);
	Wire.write(start_reg);
	Wire.endTransmission();

	Wire.requestFrom(dev_addr, len);
	
 	while(Wire.available())	 { 
		buffer[index++] = (uint8_t)Wire.read();
	}
}

